#include "armor_predict.h"
#include "../../../tool/include/util_func.h"
#include "../../../tool/include/macros.h"
#include "../autoaim_node.hpp"
#include <cmath>
using namespace std;
bool save_predictor_v(ArmorMotionDescriptior descriptior);
bool save_predict_data(double pdis, double pyaw, int64_t timestamp);

void ArmorPredictor::update(Target target, double v, bool flag_switch_armor)
{
    last_target_time = curr_target_time;
    curr_target_time = target.clock;
    double dt = (double)(curr_target_time - last_target_time) / 1000;
    TUNE_PARAM(tune_param(););
    int kf_reset = 0;
    descriptior.curr_target = target;
    // RCLCPP_INFO(autoaim_node->get_logger(), "target: %lf, %lf, %lf", target.x, target.y, target.z);
    autoaim_node->autoaim_recorder.flag_switch_armor = flag_switch_armor;
    // if ()
    // RCLCPP_INFO(autoaim_node->get_logger(), "08");
    Eigen::Matrix<double, 3, 1> xyz = {target.x, target.y, target.z};
    // RCLCPP_INFO(autoaim_node->get_logger(), "xyz: %lf, %lf, %lf", xyz[0], xyz[1], xyz[2]);
    Eigen::Matrix<double, 3, 1> ypd;
    xyz_to_ypd(xyz, ypd);
    // RCLCPP_INFO(autoaim_node->get_logger(), "0: %d, 1: %d, 2: %d, 3: %d", 2 != pre_state, 2 != post_state, std::abs(last_target_time - curr_target_time) >= 1000);
    if (2 != pre_state || 2 != post_state || flag_switch_armor || std::abs(last_target_time - curr_target_time) >= 1000)
    {
        sin_o_yaw_filter.init(3);
        cos_o_yaw_filter.init(3);
    }
    double sin_o_yaw = sin_o_yaw_filter.push_and_get_filtered_val(sin(target.o_yaw));
    double cos_o_yaw = cos_o_yaw_filter.push_and_get_filtered_val(cos(target.o_yaw));
    double tmp_yaw = atan2(sin_o_yaw, cos_o_yaw);
    autoaim_node->autoaim_recorder.pnp_z_y = tmp_yaw * 180 / M_PI;
    // RCLCPP_INFO(autoaim_node->get_logger(), "ypd: %lf, %lf, %lf", ypd[0], ypd[1], ypd[2]);
    Eigen::Matrix<double, 4, 1> z_tmp = {ypd[0], ypd[1], ypd[2], tmp_yaw};
    // std::cout << "z_tmp: " << z_tmp << std::endl;
    // tmp_yaw += M_PI;
    // tmp_yaw = atan2(cos(tmp_yaw), sin(tmp_yaw));
    // tmp_yaw *= 180 / M_PI;
    // RCLCPP_INFO(autoaim_node->get_logger(), "tmp_yaw: %lf", tmp_yaw);
    // if (std::abs(tmp_yaw) < 50)
    // {
    //     RCLCPP_INFO(autoaim_node->get_logger(), "sin=%lf, cos=%lf", sin_o_yaw, cos_o_yaw);
    // }
    // autoaim_node->autoaim_recorder.pnp_z_y = tmp_yaw;
    // RCLCPP_INFO(autoaim_node->get_logger(), "09");
    if (2 == pre_state && 2 == post_state && std::abs(last_target_time - curr_target_time) < 1000) // 如果在追踪，就更新ekf
    {
        // RCLCPP_INFO(autoaim_node->get_logger(), "11");
        // RCLCPP_INFO(autoaim_node->get_logger(), "%lf, %lf, %lf, %lf, %lf", ypd[0], ypd[1], ypd[2], target.o_yaw, dt);
        ekf.update_prior_estimate(z_tmp, dt);

        ekf.update_prior_error_cov_mat(dt);
        ekf.update_kalman_gain();
        ekf.update_posterior_estimate(z_tmp);
        ekf.update_posterior_error_cov_mat();

        queue_ekf_omega.push(ekf.X[7]);

        // RCLCPP_INFO(autoaim_node->get_logger(), "12");
        // std::cout << "update!" << std::endl;
    }
    else // 其它情况直接初始化
    {

        // RCLCPP_INFO(autoaim_node->get_logger(), "13");
        ekf.init(z_tmp);
        queue_ekf_omega.clear();
        // std::cout << "init!" << std::endl;
        // RCLCPP_INFO(autoaim_node->get_logger(), "14");
    }
    // ekf.draw_and_show_result(width, height, target.half_w, target.half_h, cameraMatrix, offset, {target.pose.ptz_pitch * M_PI / 180, target.pose.ptz_yaw * M_PI / 180, target.pose.ptz_roll * M_PI / 180, 0});

    autoaim_node->autoaim_recorder.ekf_x_center = ekf.X[0];
    autoaim_node->autoaim_recorder.ekf_v_x_center = ekf.X[1];
    autoaim_node->autoaim_recorder.ekf_y_center = ekf.X[2];
    autoaim_node->autoaim_recorder.ekf_v_y_center = ekf.X[3];
    autoaim_node->autoaim_recorder.ekf_z_center = ekf.X[4];
    autoaim_node->autoaim_recorder.ekf_v_z_center = ekf.X[5];

    std::for_each(autoaim_node->tb.enemy.begin(), autoaim_node->tb.enemy.end(), [&](auto &enemy)
                  { if(enemy.is_aimed)
    {
        Eigen::Vector3d pos = Eigen::AngleAxisd(-target.pose.ptz_yaw * M_PI / 180.-M_PI_2, Eigen::Vector3d::UnitZ()) * Eigen::Vector3d(ekf.X[0], ekf.X[2], ekf.X[4]);
        enemy.x = pos.x();
        enemy.y = pos.y();
    } });

    // double tmp_theta = ekf.X[6];
    // tmp_theta += M_PI;
    // tmp_theta = atan2(cos(tmp_theta), sin(tmp_theta));
    autoaim_node->autoaim_recorder.ekf_theta = ekf.X[6] * 180 / M_PI;
    autoaim_node->autoaim_recorder.ekf_omega = ekf.X[7] * 180 / M_PI;
    // RCLCPP_INFO(autoaim_node->get_logger(), "theta: %lf", std::abs(autoaim_node->autoaim_recorder.ekf_theta));
    // if (std::abs(autoaim_node->autoaim_recorder.ekf_theta) < 10)
    // {
    //     RCLCPP_INFO(autoaim_node->get_logger(), "ekf_theta < 10");
    // }
    autoaim_node->autoaim_recorder.ekf_r = ekf.X[8];
    // RCLCPP_INFO(autoaim_node->get_logger(), "10");
    // 在切换装甲板并且是滤波器时间与新装甲板时间差距较大时重置滤波器  //认为是不同目标机器人
    if ((flag_switch_armor && (std::abs(kalman.last_t - target.clock) > 1000)))
    {
        //        LOGM("[reset kf 1]armor switched and large delta time!");
        history_descriptiors.clear();
        Eigen::MatrixXd X(V_X, 1);
        // 速度继承
        X << target.yaw, 0.0, 0.0, target.xy_plane_distance, 0.0, 0.0, target.pitch, 0.0;
        kalman.reset(X, target.clock);
        kalman.P.setIdentity();
        kalman.K.setIdentity();
        descriptior.posterior_yaw = target.yaw;
        descriptior.posterior_xy_plane_distance = target.xy_plane_distance;
        history_descriptiors.push(descriptior);
        kf_reset = 1;
    }
    else // 在切换装甲板并且是滤波器时间与新装甲板时间差距较小时重置滤波器  //认为是同一个目标机器人
        if (flag_switch_armor && (std::abs(kalman.last_t - target.clock) < 1000))
        {
            //        LOGM("[reset kf 2]armor switched and normal delta time!");
            history_descriptiors.clear();
            Eigen::MatrixXd X(V_X, 1);
            // 速度继承
            X << target.yaw, descriptior.yaw_velocity, descriptior.yaw_acceleration, target.xy_plane_distance, descriptior.xy_plane_distance_velocity, 0, target.pitch, descriptior.pitch_velocity;

            kalman.reset(X, target.clock);
            // kalman.P.setIdentity();
            descriptior.posterior_yaw = target.yaw;
            descriptior.posterior_xy_plane_distance = target.xy_plane_distance;
            history_descriptiors.push(descriptior);
            kf_reset = 2;
        }
        // 如果没有切换装甲板并且distance没有算错很多

        else if (!flag_switch_armor && kalman.judgeDistanceDelta(target.xy_plane_distance))
        {
            //        LOGM("[update kf]no armor switched and proper distance");
            Eigen::MatrixXd Z_prior(V_Z, 1);
            Z_prior << target.yaw, target.xy_plane_distance, target.pitch;
            if (target.yaw - kalman.X(0, 0) > 150 && kalman.X(0, 0) != 0)
            {
                kalman.X(0, 0) += 360;
                //            LOGE(" change +" );
                // std::cout<<  " yaw :" <<kalman.X(0, 0)  << "  " << target.yaw << std::endl;
            }
            else if (target.yaw - kalman.X(0, 0) < -150 && kalman.X(0, 0) != 0)
            {
                kalman.X(0, 0) -= 360;
                //            LOGE(" change -" );
            }
            Eigen::MatrixXd X_posterior(V_X, 1);
            X_posterior = kalman.update(Z_prior, target.clock);

            descriptior.posterior_yaw = X_posterior(0, 0);
            descriptior.yaw_velocity = X_posterior(1, 0);
            descriptior.yaw_acceleration = X_posterior(2, 0);
            descriptior.posterior_xy_plane_distance = X_posterior(3, 0);
            descriptior.xy_plane_distance_velocity = X_posterior(4, 0);
            descriptior.xy_plane_distance_acceleration = X_posterior(5, 0);
            descriptior.posterior_pitch = X_posterior(6, 0);
            descriptior.pitch_velocity = X_posterior(7, 0);

            // geometry_msgs::msg::PoseStamped pose_msg;
            // pose_msg.pose.position.y = descriptior.posterior_yaw;
            // autoaim_node->pub_pose->publish(pose_msg);
            // geometry_msgs::msg::PoseStamped pose_msg;
            // pose_msg.header.frame_id = "pnp";
            // pose_msg.header.stamp = autoaim_node->now();
            // // pose_msg->header.stamp = 0;
            // pose_msg.pose.position.x = X_posterior(7, 0);
            // autoaim_node->pub_pose->publish(pose_msg);

            if (descriptior.yaw_velocity > 3000 || std::isnan(descriptior.posterior_yaw) || std::isnan(descriptior.posterior_xy_plane_distance))
            {
                //            LOGE("kalman velocity is error , now reset it");
                kalman.reset(target.yaw, target.xy_plane_distance, target.pitch);
                descriptior.yaw_velocity = 0;
                descriptior.yaw_acceleration = 0;
                descriptior.xy_plane_distance_velocity = 0;
            }
            history_descriptiors.push(descriptior);
            kf_reset = 3;
        } // 如果没有切换装甲板并且distance在算错，用当前的target信息重置kf
        else
        {
            //        LOGE("[reset kf 3]no armor switched and wrong distance.");
            descriptior.posterior_yaw = target.yaw;
            descriptior.yaw_velocity = 0;
            descriptior.yaw_acceleration = 0;
            descriptior.posterior_xy_plane_distance = target.xy_plane_distance;
            descriptior.xy_plane_distance_velocity = 0;
            descriptior.xy_plane_distance_acceleration = 0;
            descriptior.posterior_pitch = target.pitch;
            descriptior.pitch_velocity = 0;
            history_descriptiors.push(descriptior);
            kf_reset = 4;
        }
    // save_predictor_v(descriptior);
    autoaim_node->autoaim_recorder.kf_reset = kf_reset;
    autoaim_node->autoaim_recorder.descriptior_posterior_yaw = descriptior.posterior_yaw;
    autoaim_node->autoaim_recorder.descriptior_yaw_velocity = descriptior.yaw_velocity;
    autoaim_node->autoaim_recorder.descriptior_yaw_acceleration = descriptior.yaw_acceleration;
    autoaim_node->autoaim_recorder.descriptior_posterior_xy_plane_distance = descriptior.posterior_xy_plane_distance;
    autoaim_node->autoaim_recorder.descriptior_xy_plane_distance_velocity = descriptior.xy_plane_distance_velocity;
    autoaim_node->autoaim_recorder.descriptior_xy_plane_distance_acceleration = descriptior.xy_plane_distance_acceleration;
    autoaim_node->autoaim_recorder.descriptior_posterior_pitch = descriptior.posterior_pitch;
    autoaim_node->autoaim_recorder.descriptior_pitch_velocity = descriptior.pitch_velocity;
}

void ArmorPredictor::predTarget(double v, int64_t clock_now, SendPack &send_pack)
{
    // std::cout<<"in armor_predict mode:"<<kalman.last_t<<"clock_now:"<<clock_now<< std::endl;

    if (!if_predictor_RT(kalman.last_t, clock_now))
    {
        // RCLCPP_INFO(autoaim_node->get_logger(), "%ld, %ld", kalman.last_t, clock_now);
        // RCLCPP_INFO(autoaim_node->get_logger(), "armor predict RT failed, recently not find armor");
        //        run_1s_rate(LOGE("armor predict RT failed, recently not find armor"););
        send_pack.target_found = false;
        return;
    }
    // std::cout<<"in armor_predict mode1"<<endl;
    //  /// 由clock_now计算时间差，单位s
    //  /// clock_now 为传入predTarget时的电控时刻
    auto delta_clock = static_cast<double>(clock_now - descriptior.curr_target.clock) / 1000.0;
    // /// 根据最近的一帧装甲板后验观测数据计算子弹飞行时间
    double flight_time = 0.0;
    // cout<<descriptior.curr_target.xy_plane_distance<<endl;

    if (!Util::computBulletFlightTime(descriptior.curr_target.xy_plane_distance, descriptior.curr_target.z, v, flight_time))
    {
        send_pack.target_found = false;
        //        LOGE("get first flight time error!");
        return;
    }
    // cout<<"flight_time1:"<<flight_time<<endl;
    double pred_disance_;
    if (descriptior.curr_target.xy_plane_distance <= 5)
    {
        pred_disance_ = descriptior.posterior_xy_plane_distance + descriptior.xy_plane_distance_velocity * (flight_time + delta_clock);
        // descriptior.pre_xy_plane_distance = pred_disance_;
    }
    else if (descriptior.curr_target.xy_plane_distance > 5)
    {
        pred_disance_ = descriptior.posterior_xy_plane_distance;
        // descriptior.pre_xy_plane_distance = pred_disance_;
    }

    if (!Util::computBulletFlightTime(pred_disance_, descriptior.curr_target.z, v, flight_time))
    {
        send_pack.target_found = false;
        //        LOGE("get second flight time error!");
        return;
    }
    // std::cout << "predTarget flight_time: " << flight_time << std::endl;
    // cout<<"flight_time2:"<<flight_time<<endl;
    //  cout<<"delta_clock:"<<delta_clock<<endl;
    if (descriptior.curr_target.xy_plane_distance <= 5)
    {
        pred_disance_ = descriptior.posterior_xy_plane_distance + dis_confidence * descriptior.xy_plane_distance_velocity * (flight_time + delta_clock);
        descriptior.pre_xy_plane_distance = pred_disance_;
    }
    else if (descriptior.curr_target.xy_plane_distance > 5)
    {
        pred_disance_ = descriptior.posterior_xy_plane_distance;
        descriptior.pre_xy_plane_distance = pred_disance_;
    }

    double time;
    time = flight_time + delta_clock;
    time += clock_now;
    int64_t timestamp = clock_now + time * 1000;

    // cout<<"time is:"<<time<<",  stamp is:"<<timestamp<<endl;

    // 根据子弹飞行时间计算预测点, 单位：m, 度
    double pred_yaw_ = descriptior.posterior_yaw + v_confidence * descriptior.yaw_velocity * (flight_time + delta_clock) + a_confidence * 0.5 * descriptior.yaw_acceleration * (flight_time + delta_clock) * (flight_time + delta_clock);
    //    double pred_yaw_ = descriptior.posterior_yaw + descriptior.yaw_velocity *(flight_time + delta_clock);
    // pred_yaw_ = descriptior.curr_target.yaw;
    // cout<<"delta yaw:"<<pred_yaw_-descriptior.posterior_yaw<<endl;

    // pose_msg.header.stamp = autoaim_node->now();
    // // pose_msg.header.stamp.sec += flight_time + delta_clock;
    // pose_msg.pose.position.z = pred_yaw_;
    // autoaim_node->pub_pose->publish(pose_msg);

    // RCLCPP_INFO(autoaim_node->get_logger(), "time: %lf", flight_time + delta_clock);

    // save_predict_data(pred_disance_, pred_yaw_, time);

    /// 计算运动预测后的云台设定值
    // double pred_yaw_ = descriptior.posterior_yaw; //时间帧对齐调试
    send_pack.pred_yaw = pred_yaw_;

    auto pred_pitch_ = Util::getPitchForPTZUnderGravity(pred_disance_, descriptior.curr_target.z, v);

    // RCLCPP_INFO();

    // geometry_msgs::msg::PoseStamped pose_msg;
    // pose_msg.header.frame_id = "pnp";
    // pose_msg.header.stamp = autoaim_node->now();
    // // pose_msg->header.stamp = 0;
    // pose_msg.pose.position.x = pred_disance_;
    // pose_msg.pose.position.y = descriptior.curr_target.z;
    // pose_msg.pose.position.z = v;
    // // pose_msg.pose.orientation.x = yaw;
    // // pose_msg.pose.orientation.w = 90-true_theta * 180 / acos(-1.0);
    // autoaim_node->pub_pose->publish(pose_msg);

    send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch); // 如果计算失败则使用旧值

    // send_pack.pred_pitch=descriptior.curr_target.pitch;
    send_pack.yaw_resolution = getYawResolution(descriptior.curr_target, true);
    send_pack.pitch_resolution = getPitchResolution(descriptior.curr_target);
    /// 单位:度/s
    send_pack.yaw_palstance = descriptior.yaw_velocity;
    send_pack.pitch_palstance = descriptior.pitch_velocity;
    send_pack.target_found = true;
    // cout<<"pred_yaw_:"<<pred_yaw_<<"send_pack.pred_pitch:"<<send_pack.pred_pitch<<endl;
}

// hit_range为击打角度范围，弧度制
void ArmorPredictor::ekf_predTarget(const double &bullet_speed, const int64_t &clock_now, SendPack &send_pack, const double &hit_range, const bool &static_hit, const double& static_hit_range)
{
    if (!if_predictor_RT(this->curr_target_time, clock_now))
    {
        //        run_1s_rate(LOGE("armor predict RT failed, recently not find armor"););
        // RCLCPP_INFO(autoaim_node->get_logger(), "armor predict RT failed, recently not find armor");
        send_pack.target_found = false;
        return;
    }
    // std::cout<<"here"<<std::endl;
    auto delta_clock = static_cast<double>(clock_now - descriptior.curr_target.clock) / 1000.0;

    // cout<<descriptior.curr_target.xy_plane_distance<<endl;
    double flight_time = 0.0;

    // if (!Util::computBulletFlightTime(descriptior.curr_target.xy_plane_distance, descriptior.curr_target.z, bullet_speed, flight_time))
    // {
    //     send_pack.target_found = false;
    //     RCLCPP_INFO(autoaim_node->get_logger(), "get flight time error!");
    //     //        LOGE("get first flight time error!");
    //     return;
    // }

    cv::Mat X_c_pred;                                                                     // 预测的机器人中心及朝向角
    cv::Mat X_armor_pred;                                                                 // 预测的要击打的装甲板中心
    cv::Mat X_center_armor_pred;                                                          // 预测的中间装甲板的位置，静止击打用
    cv::Mat X_c_now = (cv::Mat_<double>(4, 1) << ekf.X[0], ekf.X[2], ekf.X[4], ekf.X[6]); // 取图时对应的机器人中心的x, y, z, o_yaw
    cv::Mat vec_x_c_now = (cv::Mat_<double>(3, 1) << -ekf.X[0], -ekf.X[2], 0);
    vec_x_c_now = vec_x_c_now / cv::norm(vec_x_c_now);
    // cv::Mat V_c = (cv::Mat_<double>(4, 1) << ekf.X[1], ekf.X[3], ekf.X[5], ekf.X[7]);       // 速度
    cv::Mat V_c = (cv::Mat_<double>(4, 1) << 0, 0, ekf.X[5], ekf.X[7]);
    cv::Mat vec_curr_target = (cv::Mat_<double>(3, 1) << -sin(ekf.X[6]), cos(ekf.X[6]), 0); // ekf中朝向角对应的向量
    cv::Mat vec_X_c_pred_O;
    double yaw_true;
    double yaw_x_c_pred;
    interfaces::msg::Target tmp_target;
    interfaces::msg::Pose tmp_pose;

    cv::FileStorage file;
    file.open(g_tune_param_path, cv::FileStorage::READ);
    double shoot_delay;
    file["shoot_delay"] >> shoot_delay;
    file.release(); // 热调参

    auto f = [&](double delta_clock, double flight_time, double &updated_filght_time) -> bool // 预测迭代函数，输入和输出单位都是秒
    {
        double dt = delta_clock + shoot_delay + flight_time;
        X_c_pred = X_c_now + V_c * dt;

        // 最初的想法，找到距离中心最近的装甲板
        // 但这样会存在问题，即当两个装甲板对称出现时很可能会左右跳变
        vec_X_c_pred_O = (cv::Mat_<double>(3, 1) << -X_c_pred.at<double>(0, 0), -X_c_pred.at<double>(1, 0), 0);
        vec_X_c_pred_O = vec_X_c_pred_O / cv::norm(vec_X_c_pred_O); // 归一化

        yaw_true = X_c_pred.at<double>(3, 0);
        double dis = __DBL_MAX__;
        int k = 0;
        for (int i = 0; i < ekf.armor_num; ++i) // 这里直接取距离此刻ekf中的o_yaw最近的装甲板击打
        {
            double dis_tmp = sqrt(pow(vec_curr_target.at<double>(0, 0) + sin(yaw_true + i * M_PI / 2), 2) + pow(vec_curr_target.at<double>(1, 0) - cos(yaw_true + i * M_PI / 2), 2));
            // double dis_tmp = sqrt(pow(vec_x_c_now.at<double>(0, 0) + sin(yaw_true + i * 2 * M_PI / ekf.armor_num), 2) + pow(vec_x_c_now.at<double>(1, 0) - cos(yaw_true + i * M_PI / 2), 2));
            // double dis_tmp = dis_tmp_1 + dis_tmp_2;
            // double dis_tmp = sqrt(pow(-1 + sin(yaw_true + i * M_PI / 2), 2) + pow(0 - cos(yaw_true + i * M_PI / 2), 2));
            if (dis_tmp < dis)
            {
                dis = dis_tmp;
                k = i;
            }
        }
        yaw_true = yaw_true + k * 2 * M_PI / ekf.armor_num;
        yaw_x_c_pred = atan2(X_c_pred.at<double>(0, 0), -X_c_pred.at<double>(1, 0));

        double delta_yaw = atan2(sin(yaw_true - yaw_x_c_pred), cos(yaw_true - yaw_x_c_pred));
        if (delta_yaw > 2 * M_PI / ekf.armor_num / 2)
        {
            yaw_true -= 2 * M_PI / ekf.armor_num;
        }
        else if (delta_yaw < -2 * M_PI / ekf.armor_num / 2)
        {
            yaw_true += 2 * M_PI / ekf.armor_num;
        }

        while (1) // 限制在-pi到pi内
        {
            if (yaw_true > M_PI)
            {
                yaw_true -= 2 * M_PI;
            }
            else if (yaw_true <= -M_PI)
            {
                yaw_true += 2 * M_PI;
            }
            else
            {
                break;
            }
        }

        X_c_pred.at<double>(3, 0) = yaw_true;

        X_armor_pred = (cv::Mat_<double>(3, 1) << X_c_pred.at<double>(0, 0) - ekf.X[8] * sin(yaw_true), X_c_pred.at<double>(1, 0) + ekf.X[8] * cos(yaw_true), X_c_pred.at<double>(2, 0));

        X_center_armor_pred = (cv::Mat_<double>(3, 1) << X_c_pred.at<double>(0, 0) - ekf.X[8] * sin(yaw_x_c_pred), X_c_pred.at<double>(1, 0) + ekf.X[8] * cos(yaw_x_c_pred), X_c_pred.at<double>(2, 0));

        if (false == static_hit)
        {
            tmp_target.x = X_armor_pred.at<double>(1, 0);
            tmp_target.y = -X_armor_pred.at<double>(0, 0);
            tmp_target.z = X_armor_pred.at<double>(2, 0);
        }
        else
        {
            tmp_target.x = X_center_armor_pred.at<double>(1, 0);
            tmp_target.y = -X_center_armor_pred.at<double>(0, 0);
            tmp_target.z = X_center_armor_pred.at<double>(2, 0);
        }
        tmp_pose.visual_time = 0;

        // ballistic_solver.set_resistance_coefficient(bullet_r, bullet_m);
        // ballistic_solver.get_result(tmp_target, bullet_speed, tmp_pose);

        // updated_filght_time = tmp_pose.visual_time;

        if (!Util::computBulletFlightTime(sqrt(pow(-tmp_target.y, 2) + pow(tmp_target.x, 2)), tmp_target.z, bullet_speed, updated_filght_time))
        {
            RCLCPP_INFO(autoaim_node->get_logger(), "get flight time error!(in f)");
            return 0;
        }
        else
        {
            return 1;
        }
        // return 1;
    };

    for (int i = 0; i < 5; ++i) // 迭代预测
    {
        if (!f(delta_clock, flight_time, flight_time))
        {
            send_pack.target_found = false;
            return;
        }
    }
    // std::cout << "total time: " << delta_clock + shoot_delay + flight_time << std::endl;
    // std::cout << "delta_clock: " << delta_clock << ", shoot delay: " << shoot_delay << ", flight_time: " << flight_time << ", total: " << delta_clock + shoot_delay + flight_time << std::endl;
    // std::cout << "delta_clock: " << delta_clock << ", dis:" << sqrt(pow(X_armor_pred.at<double>(0,0), 2)+pow(X_armor_pred.at<double>(1,0), 2)) << ", flight_time: " << flight_time << ", shoot_delay: " << shoot_delay << std::endl;
    autoaim_node->autoaim_recorder.pnp_z_x = yaw_true * 180 / M_PI;
    auto get_yaw_v = [&](void) -> double
    {
        cv::Mat omega = (cv::Mat_<double>(3, 1) << 0, 0, ekf.X[7]);
        cv::Mat r = (cv::Mat_<double>(3, 1) << X_armor_pred.at<double>(0, 0) - X_c_pred.at<double>(0, 0), X_armor_pred.at<double>(1, 0) - X_c_pred.at<double>(1, 0), 0);
        cv::Mat v = omega.cross(r);
        cv::Mat R = (cv::Mat_<double>(3, 1) << X_armor_pred.at<double>(0, 0), X_armor_pred.at<double>(1, 0), 0);
        cv::Mat v_l = v - v.dot(R) / R.dot(R) * R;
        double yaw_v = 0;
        if (std::abs(R.at<double>(0, 0)) > std::abs(R.at<double>(1, 0)))
        {
            yaw_v = v_l.at<double>(1, 0) / R.at<double>(0, 0);
        }
        else
        {
            yaw_v = -v_l.at<double>(0, 0) / R.at<double>(1, 0);
        }
        return yaw_v;
    };
    ekf_yaw_v = get_yaw_v() * 180.0 / M_PI;

    // cv::Mat vec_pred_center_target = (cv::Mat_<double>(3, 1) << -X_c_pred.at<double>(0, 0), -X_c_pred.at<double>(1, 0), 0);//预测的中间装甲板的朝向角对应的向量，静止击打用
    // if (true == static_hit)
    // {
    // double o_yaw_pred_center_target = atan2(-X_c_pred.at<double>(0, 0), X_c_pred.at<double>(1, 0));//预测的中间装甲板的朝向角，静止击打用
    // cv::Mat X_center_armor = (cv::Mat_<double>(3, 1) << );
    // }
    // else
    // {

    // }

    // RCLCPP_INFO(autoaim_node->get_logger(), "filght time: %lf", flight_time);

    send_pack.pred_yaw = atan2(tmp_target.y, tmp_target.x) * 180 / M_PI;
    auto pred_pitch_ = Util::getPitchForPTZUnderGravity(sqrt(tmp_target.x * tmp_target.x + tmp_target.y * tmp_target.y), tmp_target.z, bullet_speed);
    send_pack.pred_pitch = pred_pitch_.value_or(send_pack.pred_pitch);

    // send_pack.pred_yaw = tmp_pose.ptz_yaw * 180 / M_PI;
    // send_pack.pred_pitch = tmp_pose.ptz_pitch * 180 / M_PI;
    send_pack.yaw_resolution = getYawResolution(descriptior.curr_target, true);
    send_pack.pitch_resolution = getPitchResolution(descriptior.curr_target);
    /// 单位:度/s
    send_pack.yaw_palstance = ekf_yaw_v;
    // send_pack.pitch_palstance = descriptior.pitch_velocity;
    send_pack.pitch_palstance = 0;
    send_pack.target_found = true;

    if (false == static_hit)
    {
        double abs_dis_max = sqrt(pow(sin(yaw_x_c_pred + hit_range / 2) - sin(yaw_x_c_pred), 2) + pow(cos(yaw_x_c_pred + hit_range / 2) - cos(yaw_x_c_pred), 2));
        double abs_dis = sqrt(pow(sin(yaw_true) - sin(yaw_x_c_pred), 2) + pow(cos(yaw_true) - cos(yaw_x_c_pred), 2));
        // std::cout << "abs_dis_max: " << abs_dis_max << ", abs_dis: " << abs_dis << std::endl;
        // std::cout << "yaw_x_c_pred: " << yaw_x_c_pred * 180 / M_PI << std::endl;
        if (abs_dis < abs_dis_max) // 如果在击打区间内
        {
            send_pack.fire = true;
        }
        else
        {
            send_pack.fire = false;
        }
    }
    else // 静止击打
    {
        // std::cout << "static hit!" << std::endl;
        double dis_theta = atan2(sin(yaw_true - yaw_x_c_pred), cos(yaw_true - yaw_x_c_pred));
        // std::cout << "dis_theta: " << dis_theta << std::endl;
        if (std::abs(dis_theta) < static_hit_range / 2) // 这里的数字为容忍角度
        {
            send_pack.fire = true;
        }
        else
        {
            send_pack.fire = false;
        }
    }

}

bool ArmorPredictor::if_predictor_RT(int64_t predictor_last_update_time_stamp, int64_t clock_now)
{
    int delay_ms = 400;
    if (abs(clock_now - predictor_last_update_time_stamp) > delay_ms) /* 超过delay_ms,该直线变为无效值 */
    {
        return false;
    }
    else
    {
        return true;
    }
}
bool save_predictor_v(ArmorMotionDescriptior descriptior)
{
    // string s = vaddr;
    // string addr = "../logs/filter_"+s;
    // ofstream fout;
    // fout.open(addr,std::ios::app|std::ios::out);
    // if(!fout.is_open())
    // {
    //   LOGE("PREDICTOR V could not open!");
    //   return false;
    // }
    // fout<<descriptior.curr_target.yaw<<",";
    // fout<<descriptior.curr_target.pitch<<",";
    // fout<<descriptior.curr_target.xy_plane_distance<<",";
    // fout<<descriptior.curr_target.xyz_distance<<",";
    // fout<<descriptior.posterior_yaw<<",";
    // fout<<descriptior.posterior_pitch<<",";
    // fout<<descriptior.posterior_xy_plane_distance<<",";
    // fout<<descriptior.posterior_xyz_distance<<",";
    // fout<<descriptior.curr_target.clock<<",";
    // fout<<descriptior.yaw_velocity<<",";
    // fout<<descriptior.pitch_velocity<<",";
    // fout<<descriptior.xy_plane_distance_velocity<<",";
    // fout<<descriptior.yaw_acceleration<<endl;
    // //fout<<descriptior.xyz_distance_velocity<<endl;
    // fout.close();
    // //cout<<"finish write PREDICTOR"<<endl;
    return true;
}
bool save_predict_data(double pdis, double pyaw, int64_t timestamp)
{
    string s = vaddr;
    string addr = "../logs/predict_" + s;
    // cout<<vaddr<<endl;
    ofstream fout;
    fout.open(addr, std::ios::app | std::ios::out);
    if (!fout.is_open())
    {
        // LOGE("PREDICT FILE could not open!");
        return false;
    }

    fout << pdis << ",";
    fout << pyaw << ",";
    fout << timestamp << endl;

    fout.close();
    // cout<<"finish write predict data"<<endl;
    return true;
}
void ArmorPredictor::displayInfo()
{
}

void ArmorPredictor::show_pred(cv::Mat &canvas, const Target &prior_target, const Target &posterior_target, const Pose &pose, double v)
{
    cv::Point2i pixel_pre;
    cv::Point2i pixel_post;
    Util::back_projection(prior_target, pixel_pre, pose);
    Util::back_projection(posterior_target, pixel_post, pose);
    // 先验观测值绿色
    // RCLCPP_INFO(autoaim_node->get_logger(), "pixel_pre:%d, %d", pixel_pre.x, pixel_pre.y);
    cv::circle(canvas, pixel_pre, 5, cv::Scalar(0, 255, 0), -1, cv::LINE_8);
    // 预测值红色
    cv::circle(canvas, pixel_post, 5, cv::Scalar(0, 0, 255), -1, cv::LINE_8);
    // 当前子弹落点
    Target fall_point_;
    if (!getBulletFallPoint(pose.mcu_time, v, fall_point_))
        return;
    cv::Point2i bullet_fall_point;
    Util::back_projection(fall_point_, bullet_fall_point, pose);
    // 计算要画的大小
    double fall_point_radius = 0.0;
    double half_armor_height = 2.75e-2;
    Util::y_world_size_to_img_size(fall_point_radius, half_armor_height, sqrt(pow(fall_point_.x, 2) + (fall_point_.y, 2))); // 用平面距离不太严谨，不过影响不大，先苟着
    // std::cout << "fall_point_radius:" << fall_point_radius <<std::endl;
    cv::circle(canvas, bullet_fall_point, fall_point_radius, cv::Scalar(255, 255, 255), 2, cv::LINE_8);
}

bool ArmorPredictor::getBulletFallPoint(int64_t clock_now, double bullet_speed, Target &fall_point)
{
    double flight_time_ = 0.0;
    if (!Util::computBulletFlightTime(descriptior.curr_target.xy_plane_distance, descriptior.curr_target.z, bullet_speed, flight_time_))
    {
        return false;
    }
    int64_t past_time_stamp = clock_now - static_cast<int64_t>(flight_time_ * 1000);
    ArmorMotionDescriptior tmp_descriptor_;
    for (int i = 1; i <= history_descriptiors.length; ++i)
    {
        int index = (history_descriptiors.tail + history_descriptiors.size - i) % (history_descriptiors.size);
        if (history_descriptiors.values[index].curr_target.clock < past_time_stamp)
        {
            double yaw_ = history_descriptiors.values[index].posterior_yaw + history_descriptiors.values[index].yaw_velocity * flight_time_;
            yaw_ = (yaw_ + 90.0) * Util::PI / 180.0;
            fall_point.x = history_descriptiors.values[index].curr_target.xy_plane_distance * cos(yaw_);
            fall_point.y = history_descriptiors.values[index].curr_target.xy_plane_distance * sin(yaw_);
            fall_point.z = history_descriptiors.values[index].curr_target.z;
            return true;
        }
    }
    return false;
}

void ArmorPredictor::tune_param()
{
    cv::FileStorage file(g_tune_param_path, cv::FileStorage::READ);
    file["R_00"] >> this->kalman.R(0, 0);
    file["R_11"] >> this->kalman.R(1, 1);
    file["R_22"] >> this->kalman.R(2, 2);
    file["Q_00"] >> this->kalman.Q(0, 0);
    file["Q_11"] >> this->kalman.Q(1, 1);
    file["Q_22"] >> this->kalman.Q(2, 2);
    file["Q_33"] >> this->kalman.Q(3, 3);
    file["Q_44"] >> this->kalman.Q(4, 4);
    file["Q_55"] >> this->kalman.Q(5, 5);
    file["Q_66"] >> this->kalman.Q(6, 6);
}

bool ArmorPredictor::burst_shoot(const Pose &pose, double v, cv::RotatedRect &rect)
{

    if (!if_predictor_RT(kalman.last_t, pose.mcu_time))
    {
        // run_1s_rate(LOGM("armor predict RT failed, recently not find armor"););
        return false;
    }
    // 过去预测打出的子弹在当前帧的落点, 如果落点在当前的装甲板内，则进行爆发发弹
    // 即说明当前的过去（邻近现在）预测是正确的，所以认为现在预测也是正确的
    Target past_fall_point_;
    if (!getBulletFallPoint(pose.mcu_time, v, past_fall_point_))
        return false;
    cv::Point2i past_bullet_fall_point;
    Util::back_projection(past_fall_point_, past_bullet_fall_point, pose);
    bool if_past_contain = Util::If_rectangle_contain_point(rect, past_bullet_fall_point);
    DEBUG(
        if (if_past_contain) {
            cv::putText(canvas, "burst shoot", rect.center, cv::FONT_HERSHEY_SIMPLEX, 0.6, cv::Scalar(0, 255, 0));
        });
    return (if_past_contain);
}

ArmorPredictor::~ArmorPredictor() {}

void ArmorPredictor::get_tracker_state(const int &pre_state, const int &post_state)
{
    this->pre_state = pre_state;
    this->post_state = post_state;
}